%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 《控制之美-卷二》 代码
%% 作者：王天威，黄军魁
%% 清华大学出版社
%% 程序名称：LQR_Test_1D
%% 程序功能：离散型一维案例分析 - LQR方法 （4.4.2节案例）
%% 所用模块：[F1]反馈矩阵求解模块
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% 程序初始化，清空工作空间，缓存，
clear all;
close all;
clc;
% 读取Octave优化求解器数据库（注：如使用Matlab，可删除或注释掉本行代码）
pkg load optim;

%%%%%%%%%%%%%%%%%系统定义%%%%%%%%%%%%%%%%%%%%%
% 定义系统矩阵A
A = 1;
% 计算系统矩阵A的维度
n = size (A,1);
% 定义输入矩阵B
B = 1;
% 计算输入矩阵B的维度
p = size(B,2);

%%%%%%%%%%%%%%%%%权重设计%%%%%%%%%%%%%%%%%%%%%
% 设计系统状态权重矩阵Q，维度n x n
Q = 1;
% 设计系统终值权重矩阵S，维度n x n
S = 1;
% 设计系统输入权重矩阵R，维度p x p
R = 1;

%%%%%%%%%%%%%%%%%系统参考值%%%%%%%%%%%%%%%%%%%%
% 定义系统参考值xd
xd = 0;

%%%%%%%%%%%%%%%%%系统初始化%%%%%%%%%%%%%%%%%%%%
% 状态初始化
x0 = 1; x = x0;
% 输入初始化
u0 = 0; u = u0;

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 定义系统运行步数
k_steps = 20;
% 定义x_history零矩阵，用于储存系统状态结果，维度n x k_step
x_history = zeros(n,k_steps);
% 将系统状态初始值赋值到x_history矩阵第一个位置
x_history (:,1) = x;
% 定义u_history零矩阵，用于储存系统输入结果，维度p x k_step
u_history = zeros(p,k_steps);
% 将系统输入初始值赋值到u_history矩阵第一个位置
u_history (:,1) = u;
% 定义控制区间，对LQR控制而言，控制区间与运行步数，k_steps，一致
N = k_steps;
% 读取模块[F1]，计算系统反馈增益，F
[F] = F1_LQR_Gain(A,B,Q,R,S);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% 仿真开始，建立for循环
for k = 1 : k_steps
% 计算系统输入
u = -F * x;
% 系统输入代入系统方程，计算系统响应
x = A * x + B * u;
% 保存系统状态到预先定义矩阵的相应位置
x_history (:,k+1) = x;
% 保存系统输入到预先定义矩阵的相应位置
u_history (:,k) = u;
end

%%%%%%%%%%%%%%%%%结果%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% 结果视图：系统状态vs.运行步数
subplot  (2, 1, 1);
plot (x_history(1,:));
legend("x1")
grid on
%% 结果视图：系统输入vs.运行步数
subplot (2, 1, 2);
hold;
plot (u_history(1,:));
legend("u1")
grid on
